#include <winhack.h>
//#include <elasys_server.h>
#include <elapi.h>
#include <stdio.h>
#include <stdlib.h>

#include "camplus.h"

#define DEFAULT_SRC_BYTES   3
#define DEFAULT_WORK_BYTES  4

#define MAXSAMPLE           255
#define CENTERSAMPLE        128
#define SCALEBITS           16
#define CBCR_OFFSET         ((int) CENTERSAMPLE << SCALEBITS)
#define ONE_HALF            ((int) 1 << (SCALEBITS - 1))
#define FIX(x)              ((int) ((x) * (1L<<SCALEBITS) + 0.5))

#define R_Y_OFF             0           // offset to R => Y section
#define G_Y_OFF             (1 * (MAXSAMPLE+1)) //offset to G => Y section
#define B_Y_OFF             (2 * (MAXSAMPLE+1)) // etc.
#define R_CB_OFF            (3 * (MAXSAMPLE+1))
#define G_CB_OFF            (4 * (MAXSAMPLE+1))
#define B_CB_OFF            (5 * (MAXSAMPLE+1))
#define R_CR_OFF            B_CB_OFF        // B=>Cb, R=>Cr are the same
#define G_CR_OFF            (6 * (MAXSAMPLE+1))
#define B_CR_OFF            (7 * (MAXSAMPLE+1))
#define TABLE_SIZE          (8 * (MAXSAMPLE+1))

//  For more detail about this arithmetic below, please see the JPEG encode/decode
//  code of Grafix module.

//  Conversation equation from RGB to YCbCr
//      Y = 0.299 * R + 0.587 * G + 0.114 * B
//      Cb = -0.169 * R - 0.331 * G + 0.500 * B + 128
//      Cr = 0.500 * R -0.419 * G - 0.081 * B + 128
//  This code ueses integer table query arithmetic, for improving speed.
int *RGB2YCbCrConverter::BuildRGB2YCbCrConvertTable()
{
    int *pConvertTable;
    int i;

    pConvertTable=
        (int *) malloc(TABLE_SIZE * sizeof(int));
    if(NULL == pConvertTable){
        return NULL;
    }

    for (i = 0; i <= MAXSAMPLE; i++) {
        pConvertTable[i + R_Y_OFF] = FIX(0.299) * i;
        pConvertTable[i + G_Y_OFF] = FIX(0.587) * i;
        pConvertTable[i + B_Y_OFF] = FIX(0.114) * i + ONE_HALF;
        pConvertTable[i + R_CB_OFF] = (-FIX(0.169)) * i;
        pConvertTable[i + G_CB_OFF] = (-FIX(0.331)) * i;
        // We use a rounding fudge-factor of 0.5-epsilon for Cb and Cr.
        // This ensures that the maximum output will round to MAXSAMPLE
        // not MAXSAMPLE+1, and thus that we don't have to range-limit.
        pConvertTable[i + B_CB_OFF] = FIX(0.500) * i +
            CBCR_OFFSET + ONE_HALF - 1;
        //  B=>Cb and R=>Cr tables are the same
        //  pConvertTable[i+G_CR_OFF] =  FIX(0.50000) * i +
        //      CBCR_OFFSET + ONE_HALF - 1;
        pConvertTable[i+G_CR_OFF] = (-FIX(0.419)) * i;
        pConvertTable[i+B_CR_OFF] = (-FIX(0.081)) * i;
    }

    return pConvertTable;
}



BOOL RGB2YCbCrConverter::Initialize()
{
    m_hLock = CreateMutexW(NULL, FALSE, NULL);
    if(NULL == m_hLock){
        printf("Error: camera RGB2YCbCrConverter Initialize failed! \
        file:%s. line:%d. \n",
        __FILE__, __LINE__);
        return FALSE;
    }

    m_piConvertTable = BuildRGB2YCbCrConvertTable();
    if(NULL == m_piConvertTable){
        printf("Error: camera RGB2YCbCrConverter Initialize failed! \
        file:%s. line:%d. \n",
        __FILE__, __LINE__);
        return FALSE;
    }

    m_pRGBWorkBuf  = NULL;

    m_iDigitalZoom = 1;

    m_ulDesWidth   = 0;
    m_ulDesHeight  = 0;

    m_ulSrcWidth   = 0;
    m_ulSrcHeight  = 0;
    m_uSrcPixelBytes  = DEFAULT_SRC_BYTES;

    return TRUE;
}


void RGB2YCbCrConverter::Destroy()
{
    if(NULL != m_piConvertTable){
        free(m_piConvertTable);
        m_piConvertTable = NULL;
    }

    if(NULL != m_pRGBWorkBuf){
        free(m_pRGBWorkBuf);
        m_pRGBWorkBuf = NULL;
    }

    if(NULL != m_hLock){
        CloseHandle(m_hLock);
		m_hLock = NULL;
    }
}

BOOL RGB2YCbCrConverter::SetSrcFormat(unsigned long SrcWidth,
                    unsigned long SrcHeight,
                    unsigned int  SrcPixelBs)
{
    if((0 == SrcWidth)||(0 == SrcHeight)){
        printf("Error: camera RGB2YCbCrConverter SetSrcFormat failed! \
        file:%s. line:%d. \n",
        __FILE__, __LINE__);
        return FALSE;
    }

    WaitForSingleObject(m_hLock, (DWORD)INFINITE);

    m_ulSrcWidth   = SrcWidth;
    m_ulSrcHeight  = SrcHeight;
    if(0 != SrcPixelBs){
        m_uSrcPixelBytes = SrcPixelBs;
    }

    ReleaseMutex(m_hLock);

    return TRUE;
}

BOOL RGB2YCbCrConverter::SetDesFormat(unsigned long DesWidth,
                    unsigned long DesHeight)
{
    size_t WorkBufSize;

    if((0 == DesWidth)||(0 == DesHeight)){
        printf("Error: camera RGB2YCbCrConverter SetDesFormat failed! \
        file:%s. line:%d. \n",
        __FILE__, __LINE__);
        return FALSE;
    }

    WaitForSingleObject(m_hLock, (DWORD)INFINITE);

    m_ulDesWidth   = DesWidth;
    m_ulDesHeight  = DesHeight;

    WorkBufSize= DesWidth * DesHeight * DEFAULT_WORK_BYTES;

    // Should alloc more space than WorkBufSize, Avoiding to be overwrited by the last pixel.
    // (For example: it occupies 3 bytes, but it should write 4 bytes at a time.)
    m_pRGBWorkBuf = (BYTE *)realloc(m_pRGBWorkBuf, (WorkBufSize + 4));

    ReleaseMutex(m_hLock);
    if(NULL == m_pRGBWorkBuf){
        printf("Error: camera RGB2YCbCrConverter SetDesFormat failed! \
        file:%s. line:%d. \n",
        __FILE__, __LINE__);
        return FALSE;
    }

    return TRUE;
}

void RGB2YCbCrConverter::SetDigitalZoom(int zoom)
{
    if (zoom < 1 || zoom > DIGITALZOOM_MAX) zoom = 1;
    m_iDigitalZoom = zoom;
}

void RGB2YCbCrConverter::GetDigitalZoom(int *pZoom)
{
    *pZoom = m_iDigitalZoom;
}

void RGB2YCbCrConverter::SrcZoomToWork(BYTE *pSrcBuf)
{
    float xScale, yScale, fx, fy;
    unsigned long ZoomSrcWidth, ZoomSrcHeight; // The revised source size considering digital zoom
    unsigned long ZoomSrcPosX, ZoomSrcPosY; // The left top pixel of zoom window in the source window

    BYTE         *pDesBuf    = m_pRGBWorkBuf;
    unsigned long DesWidth   = m_ulDesWidth;
    unsigned long DesHeight  = m_ulDesHeight;

    unsigned long SrcWidth   = m_ulSrcWidth;
    unsigned long SrcHeight  = m_ulSrcHeight;
    unsigned int  SrcPixelBs = m_uSrcPixelBytes;//Reservd
    SrcPixelBs = SrcPixelBs;
    int           DigitalZoom = m_iDigitalZoom;

    ZoomSrcWidth = (SrcWidth / DigitalZoom);
    ZoomSrcHeight = (SrcHeight / DigitalZoom);

    ZoomSrcPosX = ((DigitalZoom - 1) * SrcWidth / (2 * DigitalZoom));
    ZoomSrcPosY = ((DigitalZoom - 1) * SrcHeight / (2 * DigitalZoom));

    xScale = (float)ZoomSrcWidth  / (float)DesWidth;
    yScale = (float)ZoomSrcHeight / (float)DesHeight;
    long BytesPerLine = SrcWidth * DEFAULT_SRC_BYTES;

    for(unsigned int y = 0; y < DesHeight; y++){
        fy =  SrcHeight - ZoomSrcPosY - y * yScale - 1; // Reverse order
        if (fy < 0) fy = 0; // only in the condition yScale > 1
        for(unsigned int x = 0; x < DesWidth; x++){
            fx = ZoomSrcPosX + x * xScale;

            //BUGBUG: Just support 24 bits source pixel, 32 bits destination pixel
            DWORD *pDesPixel = (DWORD *)(pDesBuf + (y * DesWidth + x) * DEFAULT_WORK_BYTES);
            DWORD *pSrcPixel = (DWORD *)(pSrcBuf + (unsigned long)fy * BytesPerLine +
                    (unsigned long)fx * DEFAULT_SRC_BYTES);
            *pDesPixel = (*pSrcPixel) & 0xFFFFFF;
        }
    }
}

void RGB2YCbCrConverter::WorkTo422R90(YCbCrBufferInfo YCbCrBuffer)
{
    BYTE *pYCbCr_Y, *pYCbCr_Cb, *pYCbCr_Cr;
    DWORD *pRGB;
    unsigned int RowCount, ColumnCount;
    int *pConvertTable;
    int RGB_R, RGB_G, RGB_B;
    unsigned int i, j;

    RowCount = m_ulDesHeight;
    ColumnCount = m_ulDesWidth;

    pConvertTable = m_piConvertTable;
    assert(pConvertTable);
    pRGB = (DWORD *)m_pRGBWorkBuf;
    pYCbCr_Y = YCbCrBuffer.pY;
    pYCbCr_Cb = YCbCrBuffer.pCb;
    pYCbCr_Cr = YCbCrBuffer.pCr;

    for (i = ColumnCount-1; i < ColumnCount; i--) {
        for (j = 0; j < RowCount; j++) {
            RGB_R = (int)((pRGB[i+j*ColumnCount] >> 16) & 0xFF);
            RGB_G = (int)((pRGB[i+j*ColumnCount] >> 8) & 0xFF);
            RGB_B = (int)(pRGB[i+j*ColumnCount] & 0xFF);
            // If the inputs are 0..MAXSAMPLE, the outputs of these equations
            // must be too; we do not need an explicit range-limiting operation.
            // Hence the value being shifted is never negative, and we don't
            // need the general RIGHT_SHIFT macro.
            *pYCbCr_Y++ =
                ((pConvertTable[RGB_R + R_Y_OFF] +
                pConvertTable[RGB_G + G_Y_OFF] +
                pConvertTable[RGB_B + B_Y_OFF])
                >> SCALEBITS);
            if (j & 0x1) {
                *pYCbCr_Cb++ =
                    ((pConvertTable[RGB_R + R_CB_OFF] +
                    pConvertTable[RGB_G + G_CB_OFF] +
                    pConvertTable[RGB_B + B_CB_OFF])
                    >> SCALEBITS);

            }
            else {
                *pYCbCr_Cr++ =
                    ((pConvertTable[RGB_R + R_CR_OFF] +
                    pConvertTable[RGB_G + G_CR_OFF] +
                    pConvertTable[RGB_B + B_CR_OFF])
                    >> SCALEBITS);
            }
        }
    }
}

void RGB2YCbCrConverter::WorkTo422(YCbCrBufferInfo YCbCrBuffer)
{
    BYTE *pYCbCr_Y, *pYCbCr_Cb, *pYCbCr_Cr;
    DWORD *pRGB;
    unsigned int RowCount, ColumnCount;
    int *pConvertTable;
    int RGB_R, RGB_G, RGB_B;
    unsigned int i, j;

    RowCount = m_ulDesHeight;
    ColumnCount = m_ulDesWidth;

    pConvertTable = m_piConvertTable;
    assert(pConvertTable);
    pRGB = (DWORD *)m_pRGBWorkBuf;
    pYCbCr_Y = YCbCrBuffer.pY;
    pYCbCr_Cb = YCbCrBuffer.pCb;
    pYCbCr_Cr = YCbCrBuffer.pCr;

    for (i = 0; i < RowCount; i++) {
        for (j = 0; j < ColumnCount; j++) {
            RGB_R = (int)(((*pRGB) >> 16) & 0xFF);
            RGB_G = (int)(((*pRGB) >> 8) & 0xFF);
            RGB_B = (int)((*pRGB) & 0xFF);
            pRGB++;
            // If the inputs are 0..MAXSAMPLE, the outputs of these equations
            // must be too; we do not need an explicit range-limiting operation.
            // Hence the value being shifted is never negative, and we don't
            // need the general RIGHT_SHIFT macro.
            *pYCbCr_Y++ =
                ((pConvertTable[RGB_R + R_Y_OFF] +
                pConvertTable[RGB_G + G_Y_OFF] +
                pConvertTable[RGB_B + B_Y_OFF])
                >> SCALEBITS);
            if (j & 0x1) {
                *pYCbCr_Cb++ =
                    ((pConvertTable[RGB_R + R_CB_OFF] +
                    pConvertTable[RGB_G + G_CB_OFF] +
                    pConvertTable[RGB_B + B_CB_OFF])
                    >> SCALEBITS);

            }
            else {
                *pYCbCr_Cr++ =
                    ((pConvertTable[RGB_R + R_CR_OFF] +
                    pConvertTable[RGB_G + G_CR_OFF] +
                    pConvertTable[RGB_B + B_CR_OFF])
                    >> SCALEBITS);
            }
        }
    }
}

BOOL RGB2YCbCrConverter::RGBTo422Rotary90(BYTE *pRGBSrcBuffer,
                    YCbCrBufferInfo YCbCrBuffer)
{
    if((NULL == pRGBSrcBuffer)||(NULL == YCbCrBuffer.pY)){
        printf("Error: camera RGB2YCbCrConverter RGBTo422Rotary90 failed! \
            file:%s. line:%d. \n",
            __FILE__, __LINE__);
        return FALSE;
    }

    if((NULL == m_piConvertTable )
        ||(NULL == m_pRGBWorkBuf)
        ||(NULL == m_hLock)
        ||(0 == m_ulDesWidth)
        ||(0 == m_ulDesHeight)
        ||(0 == m_ulSrcHeight)
        ||(0 == m_ulSrcWidth)){
        printf("Error: camera RGB2YCbCrConverter RGBTo422Rotary90 failed! Must init and set format. \
            file:%s. line:%d. \n",
            __FILE__, __LINE__);
        return FALSE;
    }

    WaitForSingleObject(m_hLock, (DWORD)INFINITE);

    SrcZoomToWork(pRGBSrcBuffer);

    WorkTo422R90(YCbCrBuffer);

    ReleaseMutex(m_hLock);

    return TRUE;
}

BOOL RGB2YCbCrConverter::RGBTo422Normal(BYTE *pRGBSrcBuffer,
                    YCbCrBufferInfo YCbCrBuffer)
{
    if((NULL == pRGBSrcBuffer)||(NULL == YCbCrBuffer.pY)){
        printf("Error: camera RGB2YCbCrConverter RGBTo422Normal failed! \
            file:%s. line:%d. \n",
            __FILE__, __LINE__);
        return FALSE;
    }

    if((NULL == m_piConvertTable )
        ||(NULL == m_pRGBWorkBuf)
        ||(NULL == m_hLock)
        ||(0 == m_ulDesWidth)
        ||(0 == m_ulDesHeight)
        ||(0 == m_ulSrcHeight)
        ||(0 == m_ulSrcWidth)){
        printf("Error: camera RGB2YCbCrConverter RGBTo422Normal failed! Must init and set format.\
            file:%s. line:%d. \n",
            __FILE__, __LINE__);
        return FALSE;
    }

    WaitForSingleObject(m_hLock, (DWORD)INFINITE);

    SrcZoomToWork(pRGBSrcBuffer);

    WorkTo422(YCbCrBuffer);

    ReleaseMutex(m_hLock);

    return TRUE;
}
